#include "speed.h"
#include "app_speed_pid.h"
#include "common.h"
#include "lcd/lcd.h"
#include "motor/hw_encoder.h"
#include "motor/hw_motor.h"
#include "pid.h"
#include "speed.h"
#include "stdio.h"
#include "string.h"
#include "utils/display.h"

// 绘制定速页界面
void speed_page_display(void) {
  TXT_OBJECT p = {0}, i = {0}, d = {0};

  // 关闭背光
  LCD_BLK_Clr();

  // 绘制全局背景
  LCD_Fill(0, 0, LCD_W, LCD_H, BLACK);

  int str_center_x = (24 * 1) / 2;

  // 显示静态的 P I D 标题
  LCD_ShowChar(screen_center_x - str_center_x - 84, 87, 'P', WHITE, BLUE, 16,
               1);
  LCD_ShowChar(screen_center_x - str_center_x, 87, 'I', WHITE, BLUE, 16, 1);
  LCD_ShowChar(screen_center_x - str_center_x + 84, 87, 'D', WHITE, BLUE, 16,
               1);

  // 显示 P 参数的圆角矩形背景
  p.start_x = screen_center_x - str_center_x - 84 - 30;
  p.start_y = 60 + 44;
  p.end_x = screen_center_x - str_center_x - 84 + 40;
  p.end_y = 60 + 44 + 24;
  LCD_ArcRect(p.start_x, p.start_y, p.end_x, p.end_y, BLUE);

  // 显示 I 参数的圆角矩形背景
  i.start_x = screen_center_x - str_center_x - 30;
  i.start_y = 60 + 44;
  i.end_x = screen_center_x - str_center_x + 40;
  i.end_y = 60 + 44 + 24;
  LCD_ArcRect(i.start_x, i.start_y, i.end_x, i.end_y, BLUE);

  // 显示 D 参数的圆角矩形背景
  d.start_x = screen_center_x - str_center_x + 84 - 30;
  d.start_y = 60 + 44;
  d.end_x = screen_center_x - str_center_x + 84 + 40;
  d.end_y = 60 + 44 + 24;
  LCD_ArcRect(d.start_x, d.start_y, d.end_x, d.end_y, BLUE);

  // 显示静态的 Speed:  Target:  标题
  LCD_ShowString(20, 170 - 34, (unsigned char *)"Speed: ", WHITE, BLUE, 24, 1);
  LCD_ShowString(320 - 150, 170 - 34, (unsigned char *)"Target: ", WHITE, BLUE,
                 24, 1);

  // 设置默认参数数据
  ui_speed_page_value_set(0, 0, 0, 0, 0, 0);

  LCD_BLK_Set(); // 打开背光
}

/*
功能：绘制定速页参数更新显示
参数：  p=当前P值
        i=当前I值
        d=当前D值
        speed=当前速度值
        target=当前目前值
        quick_update=1开启快速刷新 =0关闭快速刷新
*/
void ui_speed_page_value_set(float p, float i, float d, int32_t speed,
                             int target, int quick_update) {
  static float last_p, last_i, last_d;
  static int last_speed, last_target;

  unsigned char show_buff[50] = {0};
  TXT_OBJECT txt_p = {0}, txt_i = {0}, txt_d = {0};
  int txt_size = 0;         // 一整个字符串的像素
  int char_width_pixel = 8; // 16x16大小的英文字符实际大小是8x16
  int rect_w = 70;          // 矩形的宽度
  int rect_h = 24;          // 矩形的高度
  int rect_start_x = 34;    // 第一个矩形的起始X轴位置
  int rect_apart_x = 84;    // 矩形与矩形间的间隔

  if (quick_update != 1) // 没有开启快速更新
  {
    last_p = 65535.0;    // 设置一个不可能的值
    last_i = 65535.0;    // 设置一个不可能的值
    last_d = 65535.0;    // 设置一个不可能的值
    last_speed = 65535;  // 设置一个不可能的值
    last_target = 65535; // 设置一个不可能的值
  }

  if (last_p != p) {
    last_p = p;
    sprintf(show_buff, " %.2f ", p);
    txt_size = strlen(show_buff) * char_width_pixel;
    txt_p.start_x = rect_start_x + ((rect_w - txt_size) / 2);
    txt_p.start_y = 104 + ((rect_h - 16) / 2);
    LCD_ShowString(txt_p.start_x, txt_p.start_y, show_buff, WHITE, BLUE, 16, 0);
  }
  if (last_i != i) {
    last_i = i;
    sprintf(show_buff, " %.2f ", i);
    txt_size = strlen(show_buff) * char_width_pixel;
    txt_i.start_x = (rect_start_x + rect_apart_x) + ((rect_w - txt_size) / 2);
    txt_i.start_y = 104 + ((rect_h - 16) / 2);
    LCD_ShowString(txt_i.start_x, txt_i.start_y, show_buff, WHITE, BLUE, 16, 0);
  }
  if (last_d != d) {
    last_d = d;
    sprintf(show_buff, " %.2f ", d);
    txt_size = strlen(show_buff) * char_width_pixel;
    txt_d.start_x = (rect_start_x + rect_apart_x + rect_apart_x) +
                    ((rect_w - txt_size) / 2);
    txt_d.start_y = 104 + ((rect_h - 16) / 2);
    LCD_ShowString(txt_d.start_x, txt_d.start_y, show_buff, WHITE, BLUE, 16, 0);
  }
  if (last_speed != speed) {
    last_speed = speed;
    sprintf(show_buff, "%d ", speed);
    LCD_ShowString(12 * 6 + 20, 170 - 34, show_buff, WHITE, BLACK, 24, 0);
  }
  if (last_target != target) {
    last_target = target;
    sprintf(show_buff, "%d ", target);
    LCD_ShowString(320 - 150 + 12 * 7, 170 - 34, show_buff, WHITE, BLACK, 24,
                   0);
  }
}

// 绘制参数选择框
//  mode = 0 选择P值显示选择框
//  mode = 1 选择I值显示选择框
//  mode = 2 选择D值显示选择框
//  mode = 3 选择target值显示选择框
//  mode = 4 全部不显示选择框
void ui_speed_page_select_box(int mode) {
  char select_box_interval = 3;

  switch (mode) {
  case 0: // P
    disp_select_box(34, 104 - 34, 104, 128 - 104, 10, select_box_interval,
                    WHITE);
    disp_select_box(118, 188 - 118, 104, 128 - 104, 10, select_box_interval,
                    BLACK);
    disp_select_box(320 - 150, 316 - (320 - 150), 170 - 34, 166 - (170 - 34),
                    10, select_box_interval, BLACK);
    break;
  case 1: // I
    disp_select_box(118, 188 - 118, 104, 128 - 104, 10, select_box_interval,
                    WHITE);
    disp_select_box(202, 272 - 202, 104, 128 - 104, 10, select_box_interval,
                    BLACK);
    disp_select_box(34, 104 - 34, 104, 128 - 104, 10, select_box_interval,
                    BLACK);
    break;
  case 2: // D
    disp_select_box(202, 272 - 202, 104, 128 - 104, 10, select_box_interval,
                    WHITE);
    disp_select_box(320 - 150, 316 - (320 - 150), 170 - 34, 166 - (170 - 34),
                    10, select_box_interval, BLACK);
    disp_select_box(118, 188 - 118, 104, 128 - 104, 10, select_box_interval,
                    BLACK);
    break;
  case 3: // target
    disp_select_box(320 - 150, 316 - (320 - 150), 170 - 34, 166 - (170 - 34),
                    10, select_box_interval, WHITE);
    disp_select_box(34, 104 - 34, 104, 128 - 104, 10, select_box_interval,
                    BLACK);
    disp_select_box(202, 272 - 202, 104, 128 - 104, 10, select_box_interval,
                    BLACK);
    break;
  case 4: // all clean
    disp_select_box(34, 104 - 34, 104, 128 - 104, 10, select_box_interval,
                    BLACK);
    disp_select_box(118, 188 - 118, 104, 128 - 104, 10, select_box_interval,
                    BLACK);
    disp_select_box(202, 272 - 202, 104, 128 - 104, 10, select_box_interval,
                    BLACK);
    disp_select_box(320 - 150, 316 - (320 - 150), 170 - 34, 166 - (170 - 34),
                    10, select_box_interval, BLACK);
    break;
  }
}

/*
*   函数内容：画定速的PID波形
*   函数参数：window_start_x 波形的起始X轴坐标
              window_start_y 波形的起始Y轴坐标
              window_w 波形组件的整体宽度
              window_w 波形组件的整体高度
              curve_color 波形线的颜色
              background_color 波形组件背景色
              rawValue 波形Y轴数据
*   返回值：  当前波形的X轴坐标点
*/
uint16_t draw_speed_curve(int window_start_x, int window_start_y, int window_w,
                          int window_h, int curve_color, int background_color,
                          short int rawValue) {
  uint16_t x = 0, y = 0, i = 0;
  static char firstPoint = 1; // 是否是刚刚开始画,第一次进入
  static uint16_t lastX = 0, lastY = 0;

  // 限幅最大和最小输入值
  if (rawValue >= window_h) {
    rawValue = window_h;
  }
  if (rawValue <= 0) {
    rawValue = 0;
  }

  // 基于波形框 底部Y轴点 计算显示数据的偏移量
  y = (window_start_y + window_h) - rawValue;

  if (firstPoint) // 如果是第一次画点，则无需连线，直接描点即可
  {
    LCD_DrawPoint(window_start_x, y, curve_color);
    lastX = window_start_x;
    lastY = y;
    firstPoint = 0;
    return 0;
  }

  // 更新X轴时间线
  x = lastX + 1;

  if (x < window_w) // 不超过屏幕宽度
  {
    // 清除当前位置的内容
    LCD_DrawVerrticalLine(x, window_start_y, window_start_y + window_h,
                          background_color);
    // 在当前位置跟之前位置之间连线
    LCD_DrawLine(lastX, lastY, x, y, curve_color);
    // 下一列绘制白竖线表示X轴刷新点
    LCD_DrawVerrticalLine(x + 1, window_start_y, window_start_y + window_h,
                          WHITE);
    // 更新之前的坐标为当前坐标
    lastX = x;
    lastY = y;
  } else // 超出屏幕宽度，清屏，从第一个点开始绘制，实现动态更新效果
  {
    // 清除第一列中之前的点
    LCD_DrawVerrticalLine(window_start_x, window_start_y,
                          window_start_y + window_h, background_color);
    // 显示当前的点
    LCD_DrawPoint(window_start_x, y, curve_color);
    // 更新之前的坐标为当前坐标
    lastX = window_start_x;
    lastY = y;
  }
  return x;
}

void speed_page_handler(PageEvent event, PageParam *param) {
  static int8_t last_speed_select_box_mode = 4;
  PressEvent buttonEvent = param->data.button.buttonEvent;
  uint8_t buttonId = param->data.button.buttonId;
  switch (event) {
  case PAGE_EVENT_KEY_PRESS:
    switch (buttonEvent) {
    case SINGLE_CLICK:
      switch (buttonId) {
      case 0: // up
        switch (last_speed_select_box_mode) {
        case 0:
          get_speed_pid()->kp += 0.01f;
          break;
        case 1:
          get_speed_pid()->ki += 0.01;
          break;
        case 2:
          get_speed_pid()->kd += 0.01;
          break;
        case 3:
          get_speed_pid()->target += 1;
        }
        break;
      case 1: // down
        switch (last_speed_select_box_mode) {
        case 0:
          get_speed_pid()->kp -= 0.01f;
          get_speed_pid()->kp =
              get_speed_pid()->kp < 0.0f ? 0.0f : get_speed_pid()->kp;
          break;
        case 1:
          get_speed_pid()->ki -= 0.01;
          get_speed_pid()->ki =
              get_speed_pid()->ki < 0.0f ? 0.0f : get_speed_pid()->ki;
          break;
        case 2:
          get_speed_pid()->kd -= 0.01;
          get_speed_pid()->kd =
              get_speed_pid()->kd < 0.0f ? 0.0f : get_speed_pid()->kd;
          break;
        case 3:
          get_speed_pid()->target -= 1;
          break;
        }
        break;
      case 2:
      case 3:
        if (buttonId == 2) {
          last_speed_select_box_mode--;
        } else {
          last_speed_select_box_mode++;
        }
        if (last_speed_select_box_mode < 0)
          last_speed_select_box_mode = 4;
        if (last_speed_select_box_mode > 4)
          last_speed_select_box_mode = 0;
        ui_speed_page_select_box(last_speed_select_box_mode);
        break;
      case 4:
        if (last_speed_select_box_mode == 4) {
          pid_dynamic_clear(get_speed_pid());
          stop_motor();
          PageManager_SwitchPage(PAGE_ID_HOME);
        } else {
          motor_speed_control(get_speed_pid()->target);
        }
        break;
      }
      break;
    case DOUBLE_CLICK:
      break;
    case LONG_PRESS_START:
      break;
    case LONG_PRESS_HOLD:
      switch (buttonId) {
      case 0: // up
        switch (last_speed_select_box_mode) {
        case 0:
          get_speed_pid()->kp += 0.01f;
          break;
        case 1:
          get_speed_pid()->ki += 0.01;
          break;
        case 2:
          get_speed_pid()->kd += 0.01;
          break;
        case 3:
          get_speed_pid()->target += 1;
        }
        break;
      case 1: // down
        switch (last_speed_select_box_mode) {
        case 0:
          get_speed_pid()->kp -= 0.01f;
          get_speed_pid()->kp =
              get_speed_pid()->kp < 0.0f ? 0.0f : get_speed_pid()->kp;
          break;
        case 1:
          get_speed_pid()->ki -= 0.01;
          get_speed_pid()->ki =
              get_speed_pid()->ki < 0.0f ? 0.0f : get_speed_pid()->ki;
          break;
        case 2:
          get_speed_pid()->kd -= 0.01;
          get_speed_pid()->kd =
              get_speed_pid()->kd < 0.0f ? 0.0f : get_speed_pid()->kd;
          break;
        case 3:
          get_speed_pid()->target -= 1;
          break;
        }
        break;
      }
      break;
    }
    break;
  case PAGE_EVENT_TIMER_EXPIRE:
    break;
  case PAGE_EVENT_DATA_UPDATE:
    //绘制当前编码器数值曲线
    int curve_x = draw_speed_curve(0, 0, 319, 88, GREEN, BLACK, ( get_encoder_count() + SPEED_ENCODER_VALUE_MAX ) / SPEED_WAVEFORM_REDUCTION_FACTOR );
    //绘制目标速度的波形点
    LCD_DrawPoint(curve_x, 88 - ((get_speed_pid()->target + SPEED_ENCODER_VALUE_MAX ) / SPEED_WAVEFORM_REDUCTION_FACTOR), RED);
    ui_speed_page_value_set(get_speed_pid()->kp, get_speed_pid()->ki,
                            get_speed_pid()->kd, get_encoder_count(),
                            get_speed_pid()->target, 1);
    break;
  case PAGE_EVENT_DATA_UPDATE_MOTOR:
    motor_speed_control(get_speed_pid()->target);
    break;
  default:
    break;
  }
}

PageDescriptor speed_page = {
    .pageId = PAGE_ID_SPEED,
    .displayFunc = speed_page_display,
    .eventHandler = speed_page_handler,
};